#include "clqr_model1.h"

int main()
{
    CLQR_MODEL1 *objLQR = new CLQR_MODEL1();

    while(objLQR->index < objLQR->XRef.size()-1){
        lqrResult_t resLQR = objLQR->lqrSteerControl(objLQR->states, objLQR->refData,
                                             objLQR->pe, objLQR->pth_e,
                                             objLQR->L, objLQR->Q, objLQR->R, objLQR->dt);
        objLQR->index = resLQR.index;
        objLQR->pe = resLQR.e;
        objLQR->pth_e = resLQR.th_e;

        double a = objLQR->Kp * (objLQR->targetSpeed - objLQR->states(3))/objLQR->dt;

        vec newStates =  objLQR->updateStates(objLQR->states, a, resLQR.delta,
                                     objLQR->dt, objLQR->L, objLQR->maxSteer);
        objLQR->states = newStates;

        // 保存实际的states
        objLQR->statesActual = join_horiz(objLQR->statesActual, newStates);
    }

    objLQR->plotRes(objLQR->XRef, objLQR->YRef, objLQR->statesActual);

    delete objLQR;
    return 0;
}
